/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "iwdg.h"
#include "peripheral_init.h"
#include "main_global.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
typedef StaticTask_t osStaticThreadDef_t;
typedef StaticTimer_t osStaticTimerDef_t;
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

int left_output;
int right_output;

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for Motor_Task */
osThreadId_t Motor_TaskHandle;
uint32_t Motor_TaskBuffer[ 512 ];
osStaticThreadDef_t Motor_TaskControlBlock;
const osThreadAttr_t Motor_Task_attributes = {
  .name = "Motor_Task",
  .cb_mem = &Motor_TaskControlBlock,
  .cb_size = sizeof(Motor_TaskControlBlock),
  .stack_mem = &Motor_TaskBuffer[0],
  .stack_size = sizeof(Motor_TaskBuffer),
  .priority = (osPriority_t) osPriorityHigh,
};
/* Definitions for Usart_Task */
osThreadId_t Usart_TaskHandle;
uint32_t Usart_TaskBuffer[ 512 ];
osStaticThreadDef_t Usart_TaskControlBlock;
const osThreadAttr_t Usart_Task_attributes = {
  .name = "Usart_Task",
  .cb_mem = &Usart_TaskControlBlock,
  .cb_size = sizeof(Usart_TaskControlBlock),
  .stack_mem = &Usart_TaskBuffer[0],
  .stack_size = sizeof(Usart_TaskBuffer),
  .priority = (osPriority_t) osPriorityAboveNormal,
};
/* Definitions for ControlQueue */
osMessageQueueId_t ControlQueueHandle;
const osMessageQueueAttr_t ControlQueue_attributes = {
  .name = "ControlQueue"
};
/* Definitions for UsartQueue */
osMessageQueueId_t UsartQueueHandle;
const osMessageQueueAttr_t UsartQueue_attributes = {
  .name = "UsartQueue"
};
/* Definitions for IWDG_Timer */
osTimerId_t IWDG_TimerHandle;
osStaticTimerDef_t IWDG_TimerControlBlock;
const osTimerAttr_t IWDG_Timer_attributes = {
  .name = "IWDG_Timer",
  .cb_mem = &IWDG_TimerControlBlock,
  .cb_size = sizeof(IWDG_TimerControlBlock),
};
/* Definitions for UsartReadySem */
osSemaphoreId_t UsartReadySemHandle;
const osSemaphoreAttr_t UsartReadySem_attributes = {
  .name = "UsartReadySem"
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void MotorTask(void *argument);
void UartRxTask(void *argument);
void WatchdogCallback(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* Create the semaphores(s) */
  /* creation of UsartReadySem */
  UsartReadySemHandle = osSemaphoreNew(1, 0, &UsartReadySem_attributes);

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* Create the timer(s) */
  /* creation of IWDG_Timer */
  IWDG_TimerHandle = osTimerNew(WatchdogCallback, osTimerPeriodic, NULL, &IWDG_Timer_attributes);

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the queue(s) */
  /* creation of ControlQueue */
  ControlQueueHandle = osMessageQueueNew (1, sizeof(ControlCmd_t), &ControlQueue_attributes);

  /* creation of UsartQueue */
  UsartQueueHandle = osMessageQueueNew (5, sizeof(UsartRaw_t), &UsartQueue_attributes);

  /* USER CODE BEGIN RTOS_QUEUES */

  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of Motor_Task */
  Motor_TaskHandle = osThreadNew(MotorTask, NULL, &Motor_Task_attributes);

  /* creation of Usart_Task */
  Usart_TaskHandle = osThreadNew(UartRxTask, NULL, &Usart_Task_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  // 写入电机舵机安全默认值
  ControlCmd_t safeCmd = {.motor=0, .servo=1450};
  osMessageQueuePut(ControlQueueHandle, &safeCmd, 0, 0);


  osTimerStart(IWDG_TimerHandle, 100);  // 100ms 周期喂狗

  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_MotorTask */
/**
* @brief Function implementing the Motor_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_MotorTask */
void MotorTask(void *argument)
{
  /* USER CODE BEGIN MotorTask */

  static ControlCmd_t localCmd;
  static int left_output = 0, right_output = 0;

  /* Infinite loop */
  for(;;)
  {
    // 等待标志 0x01，阻塞直到标志被置位
    osThreadFlagsWait(0x01, osFlagsWaitAny, osWaitForever);

    /* 从队列读取最新值（不取出）*/
    if(osMessageQueueGet(ControlQueueHandle, &localCmd, NULL, 0) == osOK) {
      // 保持数据用于后续控制
    }

    get_encoder();

    float left_speed = get_left_speed();
    float right_speed = get_right_speed();

    left_output += compute_left_pid(localCmd.motor, (int)left_speed);
    right_output += compute_right_pid(localCmd.motor, (int)right_speed);

    left_output = CLAMP(left_output, -8000, 8000);
    right_output = CLAMP(right_output, -8000, 8000);

    set_motor_throttle( (int16_t)left_output,  (int16_t)right_output);

    servo_pwm_set(localCmd.servo);
  }
  /* USER CODE END MotorTask */
}

/* USER CODE BEGIN Header_UartRxTask */
/**
* @brief Function implementing the Usart_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_UartRxTask */
void UartRxTask(void *argument)
{
  /* USER CODE BEGIN UartRxTask */

  UsartRaw_t rxFrame;
  ControlCmd_t newCmd = {0};  // 局部变量避免竞态

  /* Infinite loop */
  for (;;)
  {
    // 等待信号量通知（阻塞）
    if (osSemaphoreAcquire(UsartReadySemHandle, osWaitForever) == osOK)
    {
      // 获取数据帧（可能有多个，循环处理）
      while (osMessageQueueGet(UsartQueueHandle, &rxFrame, NULL, 0) == osOK)
      {
        // 提取 motor / servo
        newCmd.motor = (uint16_t)rxFrame.data[0]*1000 +
                       (uint16_t)rxFrame.data[1]*100  +
                       (uint16_t)rxFrame.data[2]*10   +
                       (uint16_t)rxFrame.data[3];

        newCmd.servo = (uint16_t)rxFrame.data[4]*1000 +
                       (uint16_t)rxFrame.data[5]*100  +
                       (uint16_t)rxFrame.data[6]*10   +
                       (uint16_t)rxFrame.data[7];

        if (newCmd.servo < 1300) newCmd.servo = 1300;
        else if (newCmd.servo > 1600) newCmd.servo = 1600;

        // 覆盖式发送到 ControlQueue（下游控制任务读取）
        osMessageQueueReset(ControlQueueHandle);
        osMessageQueuePut(ControlQueueHandle, &newCmd, 0, 0);
      }
    }
    // // 每10秒打印栈余量
    // static uint32_t last_check = 0;
    // if (HAL_GetTick() - last_check > 10000) {
    //   printf("%s栈剩余: %d字节\n",
    //         pcTaskGetName(NULL),
    //         uxTaskGetStackHighWaterMark(NULL) * 4);
    //   last_check = HAL_GetTick();
    // }
  }
  // char buf[64];
  // snprintf(buf, sizeof(buf), "V1=%d, V2=%d\r\n", motor_value, servo_value);
  // HAL_UART_Transmit(&huart1, (uint8_t *)buf, strlen(buf), HAL_MAX_DELAY);
  /* USER CODE END UartRxTask */
}

/* WatchdogCallback function */
void WatchdogCallback(void *argument)
{
  /* USER CODE BEGIN WatchdogCallback */
  HAL_IWDG_Refresh(&hiwdg);
  /* USER CODE END WatchdogCallback */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

void TIM1_BRK_TIM9_IRQHandler(void) {
  if (__HAL_TIM_GET_FLAG(&htim9, TIM_FLAG_UPDATE)) {
    __HAL_TIM_CLEAR_FLAG(&htim9, TIM_FLAG_UPDATE);
    osThreadFlagsSet(Motor_TaskHandle, 0x01);  // 通知电机控制任务
  }
}

/* USER CODE END Application */

